
'''
####
Date : 2024-10-17
Notes: 弃用（改用VSCode更改mainwindow中的人机交互代码）
####

Modify: 
1. kata新库更换--详见self.robot_control类


####
'''

# -*- coding: utf-8 -*-



import sys
import trace
import socket
import struct
import threading
import time
import re

import modbus_tk.defines as cst

import lupa
import logging

# from processModbusTCP__ import process_Master

# C++编写的ModbusTCP客户端
from TCPmodbus_wrapper import ModbusMaster

from processModbusRTU_ import rtu_Master

from robotControl_ import robot_Control

robot_logger=logging.getLogger("processLua")
robot_logger.setLevel(logging.DEBUG)

# process_master = process_Master()
master = ModbusMaster()
rtu_master = rtu_Master()


GB_dict = {  
    f"GB{i:02d}": {"address": f"{i-1}", "value": False}  
    for i in range(1, 43)  
}  

GI_dict = {  
    f"GI{i:02d}": {"address": f"{300 + (i-1)}", "value": 0}  
    for i in range(1, 43)  
}  

GD_dict = {  
    f"GD{i:02d}": {"address": f"{400 + (i-1)*2}", "value": 0.0}  
    for i in range(1, 43)  
}

GP_dict = {  
    f"GP{i:02d}": {"address": f"{500 + (i-1)*2}", "value": 00.00}  
    for i in range(1, 43)  
} 

repository1 = []
repository2 = []
repository3 = []
repository4 = []

points = {}
robot_control = robot_Control()

class LuaProcessor():

    def __init__(self, file_path='/home/teamhd/sdk/scripts/download.lua'):
        self.lua = lupa.LuaRuntime()
        self.lua.globals()['KB'] = RobotLua  # 确保RobotLua已定义
        self.file_path = file_path
        # self.points = {}
        self.runningLua = False  # 共享标志
        self.lock = threading.Lock()  # 用于同步的锁
        self.stop_event = threading.Event()  # 用于接收停止信号的事件
        self.stopedLua = False
        self.text_dst = ''
        self.lua_thread = None

        # self.robot_control = robot_Control()

    def parse_args(self, args_str):
        return [arg.strip('"') for arg in args_str.split(',') if arg.strip()]

    def stop_process(self):
        with self.lock:
            self.runningLua = False
            self.stop_event.set()  # 设置停止事件
            if self.lua_thread and self.lua_thread.is_alive():
                self.lua_thread.kill()
                self.lua_thread.join()  # 等待线程结束
            self.stopedLua = True

    def lua_execute(self):
        try:
            with open('/home/teamhd/sdk/scripts/download.lua', 'r', encoding='utf-8') as file:
                text = file.read()
                point_pattern = r'P(\d+)\s*=\s*\[(.*?)\]'
                point_matches = re.finditer(point_pattern, text, re.DOTALL)
                for match in point_matches:
                    point_name = match.group(1)
                    coords = [float(x.strip()) for x in match.group(2).split(',')]
                    points[f'P{point_name}'] = coords
                text_delete = re.sub(r'KB\s*=\s*require\("RobotLua"\)', '', text)
                replaced_text = re.sub(r'\[', '{', text_delete)
                self.text_dst = re.sub(r'\]', '}', replaced_text)

                # # 在Lua代码中定义检查停止的函数  and not py.stop_event.is_set()
                self.lua.execute("""
                    function check_stop()
                        return py.running
                    end
                """)

                # 注册Lua回调函数
                self.lua.globals().check_stop = lambda: self.check_running()
                print(4)
                
                # 检查是否应该停止执行
                if not self.runningLua:
                    self.text_dst = ''
                    self.lua_thread.kill()

                self.runningLua = True
                self.lua_thread = thread_with_trace(target=self.lua.execute, args=(self.text_dst,))
                self.lua_thread.start()


        except Exception as e:
            print(f"lua_execute error is {e}")
    
    def check_running(self):
        with self.lock:
            return self.runningLua and not self.stop_event.is_set()

    def run(self):
        try:
            while True:
                if self.runningLua:
                    self.lua_execute()
                    time.sleep(0.1)
                    self.runningLua = False
                else:
                    # self.lua_thread.kill()
                    time.sleep(0.1) 
        except Exception as e:
            print(e)


class thread_with_trace(threading.Thread): 

    def __init__(self, *args, **keywords): 
        threading.Thread.__init__(self, *args, **keywords) 
        self.killed = False
    
    def start(self): 
        self.__run_backup = self.run 
        self.run = self.__run
        threading.Thread.start(self) 
    
    def __run(self): 
        sys.settrace(self.globaltrace) #精髓在这里，但我还没有理解为什么要这么做。。。
        self.__run_backup() 
        self.run = self.__run_backup 
    
    def globaltrace(self, frame, event, arg): 
        if event == 'call': 
            return self.localtrace 
        else: 
            return None
    
    def localtrace(self, frame, event, arg): 
        if self.killed: 
            if event == 'line': 
                raise SystemExit() 
            return self.localtrace
   
    def kill(self): 
        self.killed = True




class RobotLua:
    
    def __init__(self) -> None:
        self.Pose = []
        self.vision_tcp_socket = None

    def int_float_extern(self,a,b):
        value=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
            if value is None:
                return value
            if isinstance(value, float):
                # 精确到小数点后两位数
                return round(value , 2)
            else:
                return value
        except BaseException as e:
            robot_logger.error(e,exc_info=True)
        return value

    def transfer2Hex(self, f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    def hello(self):
        print("Hello from RobotLua")

    def WaitDI(self, di, state):
        try:
            # print(f"WaitDI di is {di}, state is {state}, type_di is {type(di)}")
            di_map = {"0": 0, "1": 1, "2": 2, "3": 3, "4": 4, "5": 5, "6": 6, "7": 7, "8": 8, "9": 9, "10": 10, "11":11}
            di_new = str(di).strip()
            if di_new in di_map:
                add = di_map[di_new] + 32
                # di_value = process_master.master_.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=add, quantity_of_x=1)
                di_value = master.read_holding_registers(1, add, 1)
                while di_value[0] != state:
                    time.sleep(0.001)
                    # di_value = process_master.master_.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=add, quantity_of_x=1)
                    di_value = master.read_holding_registers(1, add, 1)
            
            return 1

        except Exception as e:
            print(f"RobotLua class waitDI error is {e}")

    def SetDO(self, do, state):
        try:
            do_map = {"0": 0, "1": 1, "2": 2, "3": 3, "4": 4}
            do_new = str(do).strip()
            if do_new in do_map:
                #### 视觉套件---机器人控制气泵
                # if state == 1:
                #     server.vsrserver.robot_control.pump_on(is_suction=True)
                # else:
                #     server.vsrserver.robot_control.pump_off()

                #### KT-AI4 标准
                time.sleep(0.5)
                add = do_map[do_new]
                state = int(state)
                rtu_master.rtu_server.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address=12+add, output_value=state)
                robot_control.bo_out = robot_control.bo_count(robot_control.bo_out, add, value=state)
        
        except Exception as e:
            print(f"RobotLua class SetDO error is {e}")

    def SetM(self, m, state):
        try:
            m_map = {"0": 0, "1": 1, "2": 2, "3": 3, "4": 4}
            m_new = str(m).strip()
            state_map = {"0": 0, "1+": 1, "1-":2}
            state_new = str(state).strip()
            if m_new in m_map:
                #### 视觉套件--
                # if state == 1:
                #     server.vsrserver.robot_control.pump_on(is_suction=True)
                # else:
                #     server.vsrserver.robot_control.pump_off()

                #### KT-AI4 标准
                if state_new in state_map:
                    add = m_map[m_new]
                    sta = state_map[state_new]
                    rtu_master.rtu_server.execute(slave=1, function_code=cst.WRITE_MULTIPLE_REGISTERS, starting_address=16, output_value=sta)

                # add = m_map[m_new]
                # rtu_master.rtu_server.execute(slave=1, function_code=cst.WRITE_MULTIPLE_REGISTERS, starting_address=10+add, output_value=state)
        
        except Exception as e:
            print(f"RobotLua class SetM error is {e}")

    def Home(self, port):
        try:
            if port is not None:
                if port == "COM1":
                    robot_control.home_position(com_Num = 1)

                elif port == "COM2":
                    robot_control.home_position(com_Num = 2)
                
                elif port == "USB1":
                    robot_control.home_position(com_Num = 3)
        
        except Exception as e:
            print(f"RobotLua class Home error {e}")

    def MoveJ(self, port, position_name, speed_):
        try:
            # print(f"port {port}, position_name {position_name}, speed {speed_}")
            percentage = float(speed_.rstrip('%')) / 100  
            speed = 2000 * percentage  
            ### 示教点位Pose
            
            if position_name in points:
                self.Pose = points[position_name]
            
                if position_name is not None:
                    if port == "COM1":
                        robot_control.set_joint_angle(com_P = self.Pose, speed = speed, com_Num = 1)
                    
                    elif port == "COM2":
                        robot_control.set_joint_angle(com_P = self.Pose, speed = speed, com_Num = 2)
                    
                    elif port == "USB1":
                        robot_control.set_joint_angle(com_P = self.Pose, speed = speed, com_Num = 3)
            
            #### 全局变量Pose
            elif position_name in GP_dict:
                # val = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 183, quantity_of_x = 4)
                val = master.read_holding_registers(1, 183, 4)
                if val != None and val[0] != 0 or val[1] != 0 or val[2] != 0 or val[3] != 0:
                    if val[0] == 1:
                        if port == "COM1":
                            robot_control.set_joint_angle(com_P = repository1, speed = speed, com_Num = 1)
                        elif port == "COM2":
                            robot_control.set_joint_angle(com_P = repository1, speed = speed, com_Num = 2)
                    if val[1] == 2:
                        if port == "COM1":
                            robot_control.set_joint_angle(com_P = repository2, speed = speed, com_Num = 1)
                        elif port == "COM2":
                            robot_control.set_joint_angle(com_P = repository2, speed = speed, com_Num = 2)
                    if val[2] == 3:
                        if port == "COM1":
                            robot_control.set_joint_angle(com_P = repository3, speed = speed, com_Num = 1)
                        elif port == "COM2":
                            robot_control.set_joint_angle(com_P = repository3, speed = speed, com_Num = 2)
                    if val[3] == 4:
                        if port == "COM1":
                            robot_control.set_joint_angle(com_P = repository4, speed = speed, com_Num = 1)
                        elif port == "COM2":
                            robot_control.set_joint_angle(com_P = repository4, speed = speed, com_Num = 2)
                else:
                    time.sleep(0.2)
                    Pose = []
                    def int_float(a,b):
                        value=0
                        try:
                            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                            z=z0+z1 #高字节在前 低字节在后
                            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
                            if value is None:
                                return value
                            if isinstance(value, float):
                                # 精确到小数点后两位数
                                return round(value , 2)
                            else:
                                return value
                        except BaseException as e:
                            robot_logger.error(e,exc_info=True)
                        return value
                    # visual_res = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 117, quantity_of_x = 6)
                    visual_res = master.read_holding_registers(1, 117, 6)
                    for i in range (0,6,2):
                        Pose.append(int_float(visual_res[i], visual_res[i+1]))
                    x = Pose[0]
                    y = Pose[1]
                    z = Pose[2]
                    if z == 0 or z is None:
                        z = 39.5
                    else:
                        z = z
                    #### 旋转角度
                    # modbus_c = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 127, quantity_of_x = 2)
                    modbus_c = master.read_holding_registers(1, 127, 2)
                    modbus_c_new = int_float(modbus_c[0], modbus_c[1])
                    c = modbus_c_new
                    a = 0
                    b = 0

                    if port == "COM1":
                        robot_control.linear_interpolation_o(x = x, y = y, z = z, a = a, b = b, c = c, speed = speed, com_Num = 1)
                    elif port == "COM2":
                        robot_control.linear_interpolation_o(x = x, y = y, z = z, a = a, b = b, c = c, speed = speed, com_Num = 2)
                    elif port == "USB1":
                        robot_control.linear_interpolation_o(x = x, y = y, z = z, a = a, b = b, c = c, speed = speed, com_Num = 3)

        
        except Exception as e:
            print(f"RobotLua class MoveJ error is {e}")
    
    def MoveJOffs(self, port, offset_str, base_position_name, speed_):
        try:
            percentage = float(speed_.rstrip('%')) / 100  
            speed = 2000 * percentage  
            if base_position_name in points:
                self.Pose = points[base_position_name]
                print(f"MoveJOffs P is {self.Pose}")
            x, y, z = map(int, re.findall(r'\d+', offset_str))
            if port == "COM1":
                robot_control.set_joint_angle(com_P=self.Pose, com_Num=1, speed=int(speed))
                robot_control.door_interpolation(x=x, y=y, z=z, speed=int(speed), is_relative = True, com_Num=1)
            
            elif port == "COM2":
                robot_control.set_joint_angle(com_P=self.Pose, com_Num=2, speed=int(speed))
                robot_control.door_interpolation(x=x, y=y, z=z, speed=int(speed), is_relative = True, com_Num=2)
            
            elif port == "USB1":
                robot_control.set_joint_angle(com_P=self.Pose, com_Num=3, speed=int(speed))
                robot_control.door_interpolation(x=x, y=y, z=z, speed=int(speed), is_relative = True, com_Num=3)
        
        except Exception as e:
            print(f"RobotLua Class MoveJOffs ERROR is {e}")

    def MoveL(self, port, position_name, speed_):
        try:
            percentage = float(speed_.rstrip('%')) / 100  
            speed = 2000 * percentage  
            if position_name in points:
                self.Pose = points[position_name]
            if position_name is not None:
                if port == "COM1":
                    robot_control.linear_interpolation(self.Pose, speed = speed, com_Num = 1)
                elif port == "COM2":
                    robot_control.linear_interpolation(self.Pose, speed = speed, com_Num = 2)
                elif port == "USB1":
                    robot_control.linear_interpolation(self.Pose, speed = speed, com_Num = 3)
            
            #### 全局变量Pose
            elif position_name in GP_dict:
                time.sleep(0.2)
                Pose = []
                def int_float(a,b):
                    value=0
                    try:
                        z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                        z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                        z=z0+z1 #高字节在前 低字节在后
                        value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
                        if value is None:
                            return value
                        if isinstance(value, float):
                            # 精确到小数点后两位数
                            return round(value , 2)
                        else:
                            return value
                    except BaseException as e:
                        robot_logger.error(e,exc_info=True)
                    return value
                # visual_res = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 117, quantity_of_x = 6)
                visual_res = master.read_holding_registers(1, 117, 6)
                for i in range (0,6,2):
                    Pose.append(int_float(visual_res[i], visual_res[i+1]))
                x = Pose[0]
                y = Pose[1]
                z = Pose[2]
                if z == 0 or z is None:
                    z = 39.5
                else:
                    z = z
                #### 旋转角度
                # modbus_c = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 127, quantity_of_x = 2)
                modbus_c = master.read_holding_registers(1, 127, 2)
                modbus_c_new = int_float(modbus_c[0], modbus_c[1])
                c = modbus_c_new
                a = 0
                b = 0

                if port == "COM1":
                    robot_control.linear_interpolation_o(x = x, y = y, z = z, a = a, b = b, c = c, speed = speed, com_Num = 1)
                elif port == "COM2":
                    robot_control.linear_interpolation_o(x = x, y = y, z = z, a = a, b = b, c = c, speed = speed, com_Num = 2)
                elif port == "USB1":
                    robot_control.linear_interpolation_o(x = x, y = y, z = z, a = a, b = b, c = c, speed = speed, com_Num = 3)
        
        except Exception as e:
            print(f"RobotLua class MoveJ error is {e}")
    
    def MoveLOffs(self, port, offset_str, base_position_name, speed_):
        try:
            percentage = float(speed_.rstrip('%')) / 100  
            speed = 2000 * percentage  
            if base_position_name in points:
                self.Pose = points[base_position_name]
                print(f"MoveLOffs P is {self.Pose}")
            x, y, z = map(int, re.findall(r'\d+', offset_str))
            # print(f"Moving joints offset by ({x}, {y}, {z}) from {base_position_name} on {port} at speed {speed}")
            if port == "COM1":
                robot_control.linear_interpolation(self.Pose, com_Num=1, speed=int(speed))
                robot_control.door_interpolation(x=x, y=y, z=z, speed=int(speed), is_relative = True, com_Num=1)
            
            elif port == "COM2":
                robot_control.linear_interpolation(self.Pose, com_Num=1, speed=int(speed))
                robot_control.door_interpolation(x=x, y=y, z=z, speed=int(speed), is_relative = True, com_Num=2)
            
            elif port == "USB1":
                robot_control.linear_interpolation(self.Pose, com_Num=1, speed=int(speed))
                robot_control.door_interpolation(x=x, y=y, z=z, speed=int(speed), is_relative = True, com_Num=3)
        
        except Exception as e:
            print(f"RobotLua Class MoveJOffs ERROR is {e}")

    def SetVar(self, var_name, value):
        try:
            # print(f" var_name is {var_name}, value is {value} type is {type(value)}")
            def float_to_int16s(f):
                # 将浮点数打包成32位二进制数据
                b = struct.pack('f', f)
                # 将32位二进制数据拆分成两个16位二进制数据
                i1, i2 = struct.unpack('HH', b)
                return i2, i1
            if var_name in GB_dict:  
                GB_dict[var_name]["value"] = value
                value = 1 if value == True else 0
                a = int(GB_dict[var_name]["address"])
                # process_master.master_.execute(1,cst.WRITE_SINGLE_COIL,starting_address=a,output_value=value)
                master.write_single_coil(1, a, value)
            elif var_name in GI_dict:
                GI_dict[var_name]["value"] = value  
                a = int(GI_dict[var_name]["address"])
                # process_master.master_.execute(1,cst.WRITE_SINGLE_REGISTER,starting_address=a,output_value=value)
                master.write_single_register(1, a, value)
            elif var_name in GD_dict:
                GD_dict[var_name]["value"] = value  
                a = int(GD_dict[var_name]["address"])
                value_tmp = float(value)
                value_new2, val_new1 = float_to_int16s(value_tmp)
                val = [value_new2, val_new1]
                # process_master.master_.execute(1,cst.WRITE_MULTIPLE_REGISTERS,starting_address=a,output_value=val)
                master.write_multiple_registers(1, a, val)
            elif var_name in GP_dict:
                GP_dict[var_name]["value"] = value  
                a = int(GP_dict[var_name]["address"])
                value_tmp = float(value)
                value_new2, val_new1 = float_to_int16s(value_tmp)
                val = [value_new2, val_new1]
                # process_master.master_.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address = a, output_value = val)
                master.write_multiple_registers(1, a, val)

            else:  
                print(f"Variable {var_name} not found in dictionary.")  
        
        except Exception as e:
            print(f"SetVar error is {e}")

    def GetVar(self, var_name):
        if var_name in GB_dict:
            a = GB_dict[var_name]["value"]
            a = True if a == 1 else False
            return a
        elif var_name in GI_dict:
            return GI_dict[var_name]["value"]
        elif var_name in GD_dict:
            return GD_dict[var_name]["value"]
        elif var_name in GP_dict:
            return GP_dict[var_name]["value"]
        else:  
            print(f"Variable {var_name} not found in dictionary.")  
            return None  

    def VisionConnect(self, ip, port):
        try:
            self.vision_tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.vision_tcp_socket.connect((ip, int(port)))
            if ip == "0.0.0.0" or "127.0.0.1":
                ### Local Connect
                self.con_Flag = 0
            else:
                ### External Connect
                self.con_Flag = 1

        except Exception as e:
            print(f"VisioinConnect initial error is {e}")

    def VisionSend(self, data):
        try:
            # data = int(data)
            if data == "QR1" or data == "QR2" or data == "QR3" or data == "QR4" or data == "QR":
                # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 181, output_value=3)
                master.write_single_register(1, 181, 3)
                time.sleep(0.3)
                if data == "QR1":
                    ### self.actioin_flag = 31 
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=31)
                    master.write_single_register(1, 182, 31)
                elif data == "QR2":
                    ### self.actioin_flag = 32 
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=32)
                    master.write_single_register(1, 182, 32)
                elif data == "QR3":
                    ### self.actioin_flag = 33 
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=33)
                    master.write_single_register(1, 182, 33)
                elif data == "QR4":
                    ### self.actioin_flag = 34
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=34)
                    master.write_single_register(1, 182, 34)
            elif data == "A" or data == "B" or data == "C" or data == "D":   
                # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 181, output_value=4)
                master.write_single_register(1, 181, 4)
                time.sleep(1)
                if data == "A":
                    ### self.actioin_flag = 41 
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=41)
                    master.write_single_register(1, 182, 41)
                elif data == "B":
                    ### self.actioin_flag = 42
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=42)
                    master.write_single_register(1, 182, 42)
                elif data == "C":
                    ### self.actioin_flag = 43
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=43)
                    master.write_single_register(1, 182, 43)
                elif data == "D":
                    ### self.actioin_flag = 44
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=44)
                    master.write_single_register(1, 182, 44)

            elif data == "Red" or data == "Green" or data == "Blue" or data == "Yellow":
                # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 181, output_value=1)
                master.write_single_register(1, 181, 1)
                time.sleep(1)
                if data == "Red":
                    ### self.actioin_flag = 11 
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=11)
                    master.write_single_register(1, 182, 11)
                elif data == "Green":
                    ### self.actioin_flag = 12
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=12)
                    master.write_single_register(1, 182, 12)
                elif data == "Blue":
                    ### self.actioin_flag = 13
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=13)
                    master.write_single_register(1, 182, 13)
                elif data == "Yellow":
                    ### self.actioin_flag = 14
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=14)
                    master.write_single_register(1, 182, 14)

            elif data == "Triangle" or data == "Rectangle" or data == "Hexagon" or data == "Circle" :     
                # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 181, output_value=2)
                master.write_single_register(1, 181, 2)
                time.sleep(1)
                if data == "Triangle":
                    ### self.actioin_flag = 21 
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=22)
                    master.write_single_register(1, 182, 21)
                elif data == "Rectangle":
                    ### self.actioin_flag = 22
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=24)
                    master.write_single_register(1, 182, 24)
                elif data == "Hexagon":
                    ### self.actioin_flag = 23
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=23)
                    master.write_single_register(1, 182, 23)
                elif data == "Circle":
                    ### self.actioin_flag = 24
                    # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=21)
                    master.write_single_register(1, 182, 21)
            
            time.sleep(0.5)
            # process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 182, output_value=0)
            master.write_single_register(1, 182, 0)
            print(f"Sending data to vision system: {data}")

        except Exception as e:
            print(f"VisionSend error is {e}")

    def VisionReceive(self):
        try:
            visual_result = []
            visual_error = None
            visual_feature = None
            
            def int_float(a,b):
                value=0
                try:
                    z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                    z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                    z=z0+z1 #高字节在前 低字节在后
                    value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
                    if value is None:
                        return value
                    if isinstance(value, float):
                        # 精确到小数点后两位数
                        return round(value , 2)
                    else:
                        return value
                except BaseException as e:
                    robot_logger.error(e,exc_info=True)
                return value
            
            # visual_feature_tmp = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 129, quantity_of_x = 2)
            visual_feature_tmp = master.read_holding_registers(1, 129, 2)
            visual_feature = int_float(visual_feature_tmp[0], visual_feature_tmp[1])
            if self.con_Flag == 0:
                visual_error = 0
                # visual_result = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 117, quantity_of_x = 6) 
                visual_result = master.read_holding_registers(1, 117, 6)
                # for i in range (0,6,2):
                #     visual_result.append(int_float(visual_res[i], visual_res[i+1]))

                # #### 检测结果放于GP01
                time.sleep(0.1)
                # process_master.master_.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address = 500, output_value = visual_result)
                master.write_multiple_registers(1, 500, visual_result)
            
            elif self.con_Flag == 1:
                visual_error = 0
                # visual_result = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address = 117, quantity_of_x = 6) 
                visual_result = master.read_holding_registers(1, 117, 6)

                time.sleep(0.1)
                # process_master.master_.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address = 500, output_value = visual_result)
                master.write_multiple_registers(1, 500, visual_result)
            
            elif self.con_Flag == 2:
                visual_error = 1
                visual_result = []

            return visual_error, visual_result

        except Exception as e:
            print(f"VisonReceive error is {e}")

    def VisionClose(self):
        self.vision_tcp_socket.close()
        print("Closing connection to vision system")

    def WaitTime(self, seconds):
        try:
            while True:
                time.sleep(seconds)
                print(f"---- WaitTime Function execute Waiting for {seconds} seconds")
                break
        
        except Exception as e:
            print(f"WaitTime is error is {e}")

